#include "board.h"
#include <rtthread.h>
#include "lite_mb.h"
#include "lb_virtualIO.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <netdb.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/errno.h>

static lite_mb_t mb_tcp_slave = {0};

#define MAX_CLIENT_NUM 3

/*
    - 从机回调函数, 当从机接收到主机的请求(数据校验和地址功能码已经解析完)
    - 在这个回调函数内填充数据，返回数据的长度即可
*/
static int lite_mb_tcp_slave_callback(lite_mb_t* smb, int function_code, int addr, int num, void* read_write_data)
{
    int rc = 0;
    switch (function_code)
    {
        case MODBUS_FC_READ_HOLDING_REGISTERS:
        {
            if ((0 <= addr) && (addr < 125))
            {
                rc = lbo_read_hold_regs(addr, num, read_write_data);
            }
        }
        break;
    }
    if (rc < 0)
    {
        rt_kprintf("callback failed:%d\n", rc);
    }
    return rc;
}

#if (1)
static void lite_mb_tcp_slave_thread(void* param)
{
    int rc = 0;
    int count = 0;
    lite_mb_t* smb_slave = param;

    mb_init(smb_slave, MODBUS_CORE_TCP, mb_port_rtsocket_create(MODBUS_DEVICE_SLAVE, RT_LWIP_IPADDR, "8080"));

    mb_set_slave(smb_slave, 2);

    int max_fd = -1;
    int server_socket = -1;
    int client_socket[MAX_CLIENT_NUM] = {-1};

    fd_set readset;
    struct timeval select_timeout;
    select_timeout.tv_sec = 1;
    select_timeout.tv_usec = 0;

    for (int i = 0; i < MAX_CLIENT_NUM; i++)
    {
        client_socket[i] = -1;
    }
    while (1)
    {
        server_socket = mb_tcp_listen(smb_slave, MAX_CLIENT_NUM);
        rt_kprintf("mb tcp listen:%d\n", server_socket);
        while (1)
        {
            // rt_kprintf("mb loop...\n");
            max_fd = -1;
            FD_ZERO(&readset);
            FD_SET(server_socket, &readset);

            if (max_fd < server_socket)
            {
                max_fd = server_socket;
            }

            for (int i = 0; i < MAX_CLIENT_NUM; i++)
            {
                if (client_socket[i] >= 0)
                {
                    FD_SET(client_socket[i], &readset);
                    if (max_fd < client_socket[i])
                        max_fd = client_socket[i];
                }
            }

            rc = select(max_fd + 1, &readset, RT_NULL, RT_NULL, &select_timeout);
            if (rc < 0)
            {
                rt_kprintf("mb_tcp_select:%d\n", rc);
                break;
            }
            else if (rc > 0)
            {
                if (FD_ISSET(server_socket, &readset))
                {
                    int client_sock_fd = mb_tcp_accept(smb_slave, server_socket);
                    rt_kprintf("mb tcp_accept:%d\n", client_sock_fd);
                    if (client_sock_fd >= 0)
                    {
                        int index = -1;
                        for (int i = 0; i < MAX_CLIENT_NUM; i++)
                        {
                            if (client_socket[i] < 0)
                            {
                                index = i;
                                break;
                            }
                        }
                        if (index >= 0)
                        {
                            rt_kprintf("mb client index >0\n");
                            client_socket[index] = client_sock_fd;
                        }
                        else
                        {
                            rt_kprintf("mb client max :%d close:%d\n", MAX_CLIENT_NUM, client_sock_fd);
                            mb_tcp_set_socket(smb_slave, client_sock_fd);
                            mb_tcp_disconnect(smb_slave);
                        }
                    }
                }
                for (int i = 0; i < MAX_CLIENT_NUM; i++)
                {
                    if (client_socket[i] >= 0)
                    {
                        if (FD_ISSET(client_socket[i], &readset))
                        {
                            mb_tcp_set_socket(smb_slave, client_socket[i]);

                            rc = mb_slave_wait_handle(smb_slave, lite_mb_tcp_slave_callback, MODBUS_WAIT_FOREVER);
                            if (rc > 0)
                            {
                                count++;
                            }
                            else
                            {
                                if (rc == MODBUS_ERROR_READ)
                                {
                                    rt_kprintf("mb_disconnect client :%d\n", client_socket[i]);
                                    mb_tcp_set_socket(smb_slave, client_socket[i]);

                                    mb_tcp_disconnect(smb_slave);
                                    client_socket[i] = -1;
                                }
                                mb_error_recovery(smb_slave);
                            }
                        }
                    }
                    rt_thread_mdelay(100);
                }
            }
        }

        for (int i = 0; i < MAX_CLIENT_NUM; i++)
        {
            if (client_socket[i] >= 0)
            {
                rt_kprintf("mb_disconnect client :%d\n", client_socket[i]);
                mb_tcp_set_socket(smb_slave, client_socket[i]);
                mb_tcp_disconnect(smb_slave);
                client_socket[i] = -1;
            }
        }
        rt_kprintf("mb_disconnect server :%d\n", server_socket);
        mb_tcp_set_socket(smb_slave, server_socket);
        mb_tcp_disconnect(smb_slave);
        server_socket = -1;
    }
}
#endif

int lite_mb_tcp_slave(void)
{
    rt_thread_t tid;

    tid = rt_thread_create("mb_eth", lite_mb_tcp_slave_thread, &mb_tcp_slave, B_MB_TCP_COM_STACK_SIZE, B_MB_TCP_COM_THREAD_PRIORITY, 10);
    if (tid != RT_NULL)
    {
        rt_thread_startup(tid);
    }
    else
    {
        goto __exit;
    }

    return RT_EOK;

__exit:
    if (tid)
        rt_thread_delete(tid);
    return RT_ERROR;
}
